//
// Created by dyc on 25-1-4.
//

#include "param.h"



namespace hitcrt {

    /**
     * @brief 读取文件
     * @param filePath_camera camera参数文件路径
     * @param filePath_param
     * @author DongYuChen (1603692295@qq.com)
     */
    LoadParam::LoadParam(const std::string &filePath_camera, const std::string &filePath_param) {
        m_filePath_camera = filePath_camera;
        m_filePath_param = filePath_param;
        //加载文件，若路径错误，读取失败抛出异常
        try {
            m_configCamera = YAML::LoadFile(m_filePath_camera);
            m_configParam = YAML::LoadFile(m_filePath_param);
        } catch (const YAML::Exception &e) {
            std::cerr << "Error reading YAML file: " << e.what() << std::endl;
        }
    }

    LoadParam::LoadParam() {
        m_filePath_camera ="../param/camera.yaml";
        m_filePath_param = "../param/param.yaml";
        //加载文件，若路径错误，读取失败抛出异常
        try {
            m_configCamera = YAML::LoadFile(m_filePath_camera);
            m_configParam = YAML::LoadFile(m_filePath_param);
        } catch (const YAML::Exception &e) {
            std::cerr << "Error reading YAML file: " << e.what() << std::endl;
        }
    }

    /**
     *
     * @param camera 摄像机类，用于打开摄像头并进行相关设置
     * @brief 加载参数并设置摄像机类参数
     */
    void LoadParam::getCameraParam(Camera &camera) {
        const auto id = m_configCamera["CamParam"]["ID"].as<int>();
        const auto width = m_configCamera["CamParam"]["Width"].as<int>();
        const auto height = m_configCamera["CamParam"]["Height"].as<int>();
        const auto FPS = m_configCamera["CamParam"]["FPS"].as<double>();
        const auto Brightness = m_configCamera["CamParam"]["Brightness"].as<double>();
        const auto Contrast = m_configCamera["CamParam"]["Contrast"].as<double>();
        const auto Saturation = m_configCamera["CamParam"]["Saturation"].as<double>();
        const auto Auto_Exposure = m_configCamera["CamParam"]["Auto_Exposure"].as<double>();
        const auto Exposure = m_configCamera["CamParam"]["Exposure"].as<double>();
        camera.set(id,width,height,FPS,Brightness,Contrast,Saturation,Auto_Exposure,Exposure);
    }

    void LoadParam::getDetectParam(Detect &detect) {
        // const auto maxYellow = m_configParam["Detect"]["yellow"]["max"].as<std::vector<int>>();
        // detect.maxYellow = cv::Scalar(maxYellow[0],maxYellow[1],maxYellow[2]);
        // const auto minYellow = m_configParam["Detect"]["yellow"]["min"].as<std::vector<int>>();
        // detect.minYellow = cv::Scalar(minYellow[0],minYellow[1],minYellow[2]);
        // const auto maxBlack = m_configParam["Detect"]["black"]["max"].as<std::vector<int>>();
        // detect.maxBlack = cv::Scalar(maxBlack[0],maxBlack[1],maxBlack[2]);
        // const auto minBlack = m_configParam["Detect"]["black"]["min"].as<std::vector<int>>();
        // detect.minBlack = cv::Scalar(minBlack[0],minBlack[1],minBlack[2]);
        // const auto maxBlue = m_configParam["Detect"]["blue"]["max"].as<std::vector<int>>();
        // detect.maxBlue = cv::Scalar(maxBlue[0],maxBlue[1],maxBlue[2]);
        // const auto minBlue = m_configParam["Detect"]["blue"]["min"].as<std::vector<int>>();
        // detect.minBlue = cv::Scalar(minBlue[0],minBlue[1],minBlue[2]);
        // const auto maxRed_1 = m_configParam["Detect"]["red_1"]["max"].as<std::vector<int>>();
        // detect.maxRed_1 = cv::Scalar(maxRed_1[0],maxRed_1[1],maxRed_1[2]);
        // const auto minRed_1 = m_configParam["Detect"]["red_1"]["min"].as<std::vector<int>>();
        // detect.minRed_1 = cv::Scalar(minRed_1[0],minRed_1[1],minRed_1[2]);
        // const auto maxRed_2 = m_configParam["Detect"]["red_2"]["max"].as<std::vector<int>>();
        // detect.maxRed_2 = cv::Scalar(maxRed_2[0],maxRed_2[1],maxRed_2[2]);
        // const auto minRed_2 = m_configParam["Detect"]["red_2"]["min"].as<std::vector<int>>();
        // detect.minRed_2 = cv::Scalar(minRed_2[0],minRed_2[1],minRed_2[2]);
        //
        // detect.IF_MASK_PROCESS = m_configParam["Detect"]["mask_process"].as<int>();
        // detect.MASK_PROCESS_ITERATION = m_configParam["Detect"]["mask_process_iterations"].as<int>();

    }

    void LoadParam::getCamFixParam(cv::Mat &cameraMatrix, cv::Mat &distCoeffs) {
        //获取相机内参
        const auto fx = m_configCamera["CameraFix"]["camera_matrix"]["fx"].as<double>();
        const auto fy = m_configCamera["CameraFix"]["camera_matrix"]["fy"].as<double>();
        const auto cx = m_configCamera["CameraFix"]["camera_matrix"]["cx"].as<double>();
        const auto cy = m_configCamera["CameraFix"]["camera_matrix"]["cy"].as<double>();
        const auto k1 = m_configCamera["CameraFix"]["distortion_coefficients"]["k1"].as<double>();
        const auto k2 = m_configCamera["CameraFix"]["distortion_coefficients"]["k2"].as<double>();
        const auto k3 = m_configCamera["CameraFix"]["distortion_coefficients"]["k3"].as<double>();
        const auto p1 = m_configCamera["CameraFix"]["distortion_coefficients"]["p1"].as<double>();
        const auto p2 = m_configCamera["CameraFix"]["distortion_coefficients"]["p2"].as<double>();
        cameraMatrix = (cv::Mat_<double>(3,3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
        distCoeffs = (cv::Mat_<double>(1,5) << k1, k2, p1, p2, k3);
    }

    YAML::Node LoadParam::getControlParam() {
        return m_configParam;
    }

}
